﻿using DotLiquid.Util;
using OfficeOpenXml.FormulaParsing.Excel.Functions.Information;
using System;
using System.IO;
using System.IO.Ports;
using System.Net;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Timers;
using System.Windows.Forms;

namespace WindowsFormsApp2

{

    public partial class 六轴控制 : Form

    {
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll")]
        public static extern void board_pnp( int  length);
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern float get_data0();
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern float get_data1();
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern float get_data2();
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern float get_data3();
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern float get_data4();
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern float get_data5();
        [DllImport("D:\\opencv\\dll_pnp-master\\x64\\Release\\dll_pnp.dll", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool is_detected();

        // 识别4*4的棋盘格并在控制台打印棋盘格的中心相对于相机中心的相对位姿。length为棋盘格每个方格的边长，单位mm

        public 六轴控制()
        {

            //定时器(新沾)
          //  UltraHighAccurateTimer timer1 = new UltraHighAccurateTimer();
            InitializeComponent();
            SineTimer.Elapsed += new System.Timers.ElapsedEventHandler(SineTimeEvent);
            SineTimer.AutoReset = true;
            AnysportTimer .Elapsed+=new System.Timers.ElapsedEventHandler(AnysportTimeevent );//定时器设置1（六轴控制定时器）
            AnysportTimer .AutoReset = true;
            duqukongzhitaiTimer.Elapsed += new System.Timers.ElapsedEventHandler(duqukongzhitaiTimeevent);//定时器设置1（六轴控制定时器）
            duqukongzhitaiTimer.AutoReset = true;
                  
            Control.CheckForIllegalCrossThreadCalls = false;//忽略跨线程调用
           

            //新沾化
            //timer1.Tick += new UltraHighAccurateTimer.ManualTimerEventHandler(duqukongzhitai);
            //timer1.Interval = 100;
            //timer1.Start();
            
            //串口初始化(串口通信必要的)
            Initialsettings();
            System.Windows.Forms.Control.CheckForIllegalCrossThreadCalls = false;
          
        }
        System.Timers.Timer duqukongzhitaiTimer = new System.Timers.Timer();//定时器设置（2）
       
       //多线程：
        private void button9_Click(object sender, EventArgs e)
        {
            int length = 12;
            Thread th = new Thread(new ThreadStart(delegate { board_pnp(length); }));//多线程相机输入
            th.IsBackground = true;
            th.Start();
        }
        private void button12_Click(object sender, EventArgs e)//读取控制台数据
        {
            
            //StringWriter sw = new StringWriter();
            //StringReader strReader// = new StringReader(textReaderText);
            //Console.SetOut(sw);
            //string sw1 = Convert.ToString(sw);
            //shijuejuzhen.AppendText(sw1);
            duqukongzhitaiTimer.Interval = 500; //0.01*1000+0.5  定时器发生时间的间隔越小采样频率越高 定时器 (3)                                                   //定时器使能
            duqukongzhitaiTimer.Enabled = true;

        }
        double ROLLP1 = 0;//中间变量角度
        double YAWP1 = 0;//中间变量角度
        float XP1 = 0;//中间变量
        float YP1 = 0;//中间变量
        private void duqukongzhitaiTimeevent(object sender, ElapsedEventArgs e)
        {

            //float data = get_data();
            bool status = is_detected();
            float[] data = new float[6];
            data[0] = get_data0();
            data[1] = get_data1();
            data[2] = get_data2();
            data[3] = get_data3();
            data[4] = get_data4();
            data[5] = get_data5();
            //for (int k = 0; k < 6; k++)
            //    richTextBox1.AppendText(in_data[k].ToString());
            //for (int i = 0; i < 6; i++)
            //    shijuejuzhen.AppendText(data[i].ToString() + "\n");
            // shijuejuzhen.AppendText(data[3].ToString() + "\n\r");
            //shijuejuzhen.AppendText(status.ToString() + "\n");
            // shijuejuzhen.AppendText("\n\r");
            //显示姿态
            float a4 = data[4] - 10.96752f;
            float a3 = data[3] - 3.547041f;
            textBoxY.AppendText(a4.ToString ());
            textBoxY.AppendText(System.Environment.NewLine);//换行左右 
            textBoxX.AppendText(a3.ToString());
            textBoxX.AppendText(System.Environment.NewLine);//换行上下
            textBoxZ.AppendText(data[5].ToString());
            textBoxZ.AppendText(System.Environment.NewLine);//换行
            textBoxα.AppendText(data[0].ToString());
            textBoxα.AppendText(System.Environment.NewLine);//换行
            textBoxβ.AppendText(data[1].ToString());//是绕y
            textBoxβ.AppendText(System.Environment.NewLine);//换行
            textBoxγ.AppendText(data[2].ToString());
            textBoxγ.AppendText(System.Environment.NewLine);//换行




            UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
            UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
            UdpAppData.I32PlayLine += 1;
            UdpAppData.I32PlayTime = 500;//时间单位为毫秒  是指令的执行时间执行时间越长动作越慢

            //            float q = Convert.ToSingle(str);//将传感器传来的数据转换为浮点型。
            //            float force2 = (float)(250 * (q - 5.4));//电压信号转力
            //            float force = Math.Abs(force2);//显示力
            //            force1 = Convert.ToString(force);
            //;
            //                double YP1 = (250 * (q - 5.4)/*力*// 5);

            float XP2 = 30;  //  (Math .Abs (data[4] - 2.96752f) < 3 ? 0 :- (data[4] - 2.96752f));//上下
                         //去除野值
            if (XP2 > 80)
            {
                XP2 = 0;
            }
            else if (XP2< -80)
            {
                XP2 = 0;
            }
             //XP2 = XP;//yawp2是摄像头测得的相对姿态角
            float  XP = XP2 + XP1;//yawp1上一时刻的姿态角绝对姿态角，yawp时输入电钢的绝对位置
                           //限位
            if (XP > 50)
            {
                XP = 50;
            }
            else if (XP < -50)
            {
                XP = -50;

            }


            // XP = XP + XP1;
            //float XP = data[3] - 3.547041f;  //  l[0]                //相对初始位置运动的坐标
            //float YP = data[4] - 10.96752f;
            float YP2 =  (Math .Abs (data[3] +15.73071f)< 3 ? 0:-(data[3]  +15.73071f));//左右
            //去除野值
            if (YP2 > 75)
            {
                YP2 = 0;
            }
            else if (YP2 < -75)
            {
                YP2 = 0;
            }
            //float  YP2 = YP;//yawp2是摄像头测得的相对姿态角
            float YP = YP2 + YP1;//yawp1上一时刻的姿态角绝对姿态角，yawp时输入电钢的绝对位置
          //限位
            if (YP > 70 )
            {
                YP = 70;
                

            }
            else if (YP < -70)
            {
                YP = -70;
              

            }


           
            // textBox2.AppendText(str.Length == 1 ? "0" + str : str + " ");
            float ZP = 50;
           
            double ROLLP2 =  (Math.Abs(data[1]+3.725197f) * Math.PI / 180 * Math.PI / 180 < 6 ? 0 : -(data[1] + 3.725197) * Math.PI / 180); ;            //相对静平台的恣态绕Y轴
            if (ROLLP2 > 40 * Math.PI / 180)
            {
                ROLLP2 = 0;
            }
            else if (ROLLP2< -40 * Math.PI / 180)
            {
                ROLLP2 = 0;
            }
            double ROLLP = ROLLP1 + ROLLP2;//ROLLP1上一时刻的姿态角绝对姿态角，yawp时输入电钢的绝对位置
            if (ROLLP > 40 * Math.PI / 180)
            {
                ROLLP2= 0;
            }
            else if (ROLLP< -40 * Math.PI / 180)
            {
                ROLLP = 0;
            }
            double PITCHP = 0;
            double YAWP2 = 0;//(Math.Abs(data[2] + 87.56854) * Math.PI / 180 < 4 * Math.PI / 180 ? 0 : (data[2] + 87.56854) * Math.PI / 180);
            //判断摄像头的野值问题
            if (YAWP2 > 40 * Math.PI / 180)
            {
                YAWP2 = 0;
            }
            else if (YAWP2 < -40 * Math.PI / 180)
            {
                YAWP2 = 0;
            }

            //double YAWP2 = YAWP;//yawp2是摄像头测得的相对姿态角
            double  YAWP = YAWP2 + YAWP1;//yawp1上一时刻的姿态角绝对姿态角，yawp时输入电钢的绝对位置
         //限位
            if (YAWP > 20 * Math.PI / 180)
            {
                YAWP = 20 * Math.PI / 180; 
                
            }
            else if (YAWP < -20 * Math.PI / 180)
            {
                YAWP = -20 * Math.PI / 180;
               
            }
           


            float[] m = { XP, YP, ZP, (float)PITCHP, (float)ROLLP, (float)YAWP };
            
            
            
            
            nijie = FANJIE(m);
            // float  z=UdpAppData.I32PlayWpos;
            //float p1 = z - 50000;//第一版(第一组)
            //float  p1= (float)(z -(100000*(q-4.13)));//第二版（第二组）这是

            UdpAppData.I32PlayXpos = (int)(2000 * nijie[0]);//绝对位置
            UdpAppData.I32PlayYpos = (int)(2000 * nijie[1]);
            UdpAppData.I32PlayZpos = (int)(2000 * nijie[2]);
            UdpAppData.I32PlayUpos = (int)(2000 * nijie[3]);
            UdpAppData.I32PlayVpos = (int)(2000 * nijie[4]);
            UdpAppData.I32PlayWpos = (int)(2000 * nijie[5]);

            PlayActionCmd();
          

            YAWP1 = YAWP2 +YAWP1 ;
            XP1 = XP2 + XP1;
            YP1 =YP2 + YP1;
            if (YP1 > 70)
            {
                YP1 = 70;
            }
            else if (YP1 < -70)
            {
                YP1 =- 70;
            }
            if (XP1 > 70)
            {
                XP1 = 70;
            }
            else if (XP1 < -70)
            {
                XP1 = -70;
            }
        }

        //停止图像采集
        private void button13_Click(object sender, EventArgs e)
        {
            duqukongzhitaiTimer.Enabled = false;
        }


        private void Initialsettings()//串口设置
        {
            //三维力串口设置
            string[] ports = SerialPort.GetPortNames();
            if (ports.Length > 0)
            {
                cmb_port.DataSource = ports;//获得端口号
                cmb_port.SelectedIndex = 0;
            }

            string[] bauds = new string[] { "自定义", "4800", "9600", "115200" };
            cmb_baud.DataSource = bauds;//获取波特率；
            cmb_baud.SelectedIndex = 3;
           

            int[] data = new int[] { 5, 6, 7, 8 };
            cmb_databits.DataSource = data;//获取数据；
            cmb_databits.SelectedIndex = 3;

            cmb_parity.DataSource = Enum.GetNames(typeof(Parity));
            cmb_parity.SelectedIndex = 0;//校验位

            cmb_stopbits.DataSource = Enum.GetNames(typeof(StopBits));
            cmb_stopbits.SelectedIndex = 1;//停止位
  //-----------------------------------------------------------------------

            //视觉传感器串口设置
            string[] ports1 = SerialPort.GetPortNames();
            if (ports1.Length > 0)
            {
                cmb_port1.DataSource = ports1;//获得端口号
                cmb_port1.SelectedIndex = 0;
            }

            string[] bauds1 = new string[] { "自定义", "4800", "9600", "115200" };
            cmb_baud1.DataSource = bauds1;//获取波特率；
            cmb_baud1.SelectedIndex = 3;


            int[] data1 = new int[] { 5, 6, 7, 8 };
            cmb_databits1.DataSource = data1;//获取数据；
            cmb_databits1.SelectedIndex = 3;

            cmb_parity1.DataSource = Enum.GetNames(typeof(Parity));
            cmb_parity1.SelectedIndex = 0;//校验位

            cmb_stopbits1.DataSource = Enum.GetNames(typeof(StopBits));
            cmb_stopbits1.SelectedIndex = 1;//停止位

        }//串口设置（第一步）（每个传感器都一样）

        //三维力端口初始化
        private SerialPort serialPort = new SerialPort();//（第二步）
        //这是三维力串口连接//（第三步）
        private void lianjie_Click_1(object sender, EventArgs e)
        {
            try
            {
                serialPort.PortName = cmb_port.Text.Trim();
                serialPort.BaudRate = Convert.ToInt32(cmb_baud.Text.Trim());//trim是去掉空格
                serialPort.DataBits = Convert.ToInt32(cmb_databits.Text.Trim());
                serialPort.Parity = (Parity)Enum.Parse(typeof(Parity), cmb_parity.Text.Trim());
                serialPort.StopBits = (StopBits)Enum.Parse(typeof(StopBits), cmb_stopbits.Text.Trim());
                serialPort.Open();
                MessageBox.Show("串口打开成功");
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        
        }
        
        //点了三维力那个接收数据的按钮
        private void button5_Click(object sender, EventArgs e)//接收电压传感器
        {
            serialPort.DataReceived += new SerialDataReceivedEventHandler(port_datareceived);
            chuankou = true;
            forcereceive = true;
            gangchangshujujieshou = true;

        }

        //创建接收三维力传感器数据的事件
        private void port_datareceived(object sender, SerialDataReceivedEventArgs e)
        {
            if (chuankou)
            {
                if (radioButton4.Checked)//如果接收方式是字符串
                {
                    str = serialPort.ReadExisting();//字符串读取
                    //* textBox2.AppendText(str);//尾部添加
                    textBox2.AppendText(str);
                }
                else
                {
                    byte data;
                    data = (byte)serialPort.ReadByte();//将int类型的数据强项转换byte
                    string str = Convert.ToString(data, 16); //转换大写16进制
                    textBox2.AppendText(str.Length == 1 ? "0" + str : str + " ");

                }
                if (forcereceive)
                {
                    string force3 = string.Format("{0:N3}", force1);//保留两位小数
                    textBox3.AppendText(force3);
                    textBox3.AppendText(System.Environment.NewLine);
                }
               
            }




        }

        //停止接收三维力‘缸长的数据额

        public static string str;
        bool chuankou ; //点击停止所有都收不到数据
        bool gangchangshujujieshou ;
        bool forcereceive;
        private void button6_Click(object sender, EventArgs e)
        {
            chuankou = false;//停止接收数据
            gangchangshujujieshou = false;//停止接收
            forcereceive = false;//力停止
        }
        //——————————————————————————————————

        private  SerialPort serport = new SerialPort();//创建视觉端口初始化（第二步）

        //这是视觉传感器端口连接(第三步)
        private void button7_Click(object sender, EventArgs e)
        {
            try
            {
                serport .PortName = cmb_port1.Text.Trim();
                serport .BaudRate = Convert.ToInt32(cmb_baud1.Text.Trim());//trim是去掉空格
                serport .DataBits = Convert.ToInt32(cmb_databits1.Text.Trim());
                serport .Parity = (Parity)Enum.Parse(typeof(Parity), cmb_parity1.Text.Trim());
                serport .StopBits = (StopBits)Enum.Parse(typeof(StopBits), cmb_stopbits1.Text.Trim());
                serport .Open();
                MessageBox.Show("串口打开成功");
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
            
        }

        //接收数据按钮（第四步）
        private void button8_Click(object sender, EventArgs e)
        {
            serport.DataReceived += new SerialDataReceivedEventHandler(serport_datareceived);
        }

        //byte[] revdata;
        //private StringBuilder strbuilder = new StringBuilder();//定义可变字符变量用于储存串口收到数据
        ////接收数据事件(第五步)
        private void serport_datareceived(object sender, SerialDataReceivedEventArgs e)
        {
            //int revdatanum = serport.BytesToRead;//获取串口接收缓存区的字节数；
            //revdata = new byte[revdatanum];//初始化字节组；
            //serport.Read(revdata, 0, revdatanum);//从串口中读取获得的数据存在revdata中；
            //strbuilder.Remove(0, strbuilder.Length);//每次都清空strbuilder中字符串的内容并存放最近一次所接收的数据
            ////因为c#中多线程处理，非别为加载窗口、和其他的文本框，如果要显示其他文本框的内容需要委托。
            //this.Invoke((EventHandler)(delegate
            //{
            //    strbuilder.Append(Encoding.Default.GetString(revdata));//将字节组revdata中的数据转换到字符串变量strbuiler中；
            //    this.richTextBox1.AppendText(strbuilder.ToString() + "\n");
            //}));
            //string[] strshijue = strbuilder.ToString().Split(' ');
            //if (strshijue.Length == 18 && strshijue[0].Trim() == "55" && strshijue[17].Trim() == "")
            //{
            //    this.Invoke((EventHandler)(delegate
            //   {
                   
            //   }));
            //}

            
        
       }
        //视觉接收数据

        private void Form1_Load(object sender, EventArgs e)
        {

        }

        //以上是三维力、视觉com端口获取数据
        class UDP_APP_DATA
        {
            public short I16HostTxPort = 8410;
            public short I16HostRxPort = 8410;
            public short I16MboxTxPort = 7408;
            public short I16MboxRxPort = 7408;

            public ushort I16ConfirmCode = 0x55aa;//确认码
            public ushort I16PassCode = 0x0000;//通过码
            public ushort I16FunctionCode = 0x1401;//功能码：相对对时间码
            public ushort I16ChannelCode = 0x0001;//通道号：六轴模式

            public ushort I16WhoAcceptCode = 0xffff;//谁接收
            public ushort I16WhoReplyCode = 0xffff;//谁回复

            public int I32PlayLine = 0x00000000;//指令编号
            public int I32PlayTime = 0x00000000;//时间（单位：毫秒，1401相对时间为指令的执行时间，1301绝对时间下相邻两帧指令里的时间相减为执行时间）
            public int I32PlayXpos = 0x00000000;//第1轴位置信息
            public int I32PlayYpos = 0x00000000;//第2轴位置信息
            public int I32PlayZpos = 0x00000000;//第3轴位置信息
            public int I32PlayUpos = 0x00000000;//第4轴位置信息
            public int I32PlayVpos = 0x00000000;//第5轴位置信息
            public int I32PlayWpos = 0x00000000;//第6轴位置信息

            public ushort I16BaseDoutCode = 0x0000;//基础开关量输出

            public ushort I16DacOneCode = 0x0000;//第1路模拟量输出
            public ushort I16DacTwoCode = 0x0000;//第2路模拟量输出
            public ushort I16ExtDoutCode = 0x0000;//扩展12路模拟量输出
        }
        public UdpClient HostUDPPort = new UdpClient(8410);//打开8410端口
        public short[] I16UdpTxBuffer = new short[25];//存放UDP数据信息 
        public byte[] U8UdpDataSend = new byte[50];//存放发送数据Byte信息    
                                                   //定时器

        UDP_APP_DATA UdpAppData = new UDP_APP_DATA();//创建实体变量 
                                                     //将两个char数据合并为字节数据
        private int ByteToWord(int x, int y)
        {

            int z;
            z = ((x << 8) | y);
            return z;
        }

        //将short类型数据的高低位进行交换
        private ushort Swap(ushort x)
        {
            ushort highbit;
            ushort lowbit;
            ushort swap;
            highbit = (ushort)(x >> 8);
            lowbit = (ushort)((x & 0x00ff) << 8);
            swap = (ushort)(highbit | lowbit);
            return swap;
        }

        ///取Uint32的高１６位，并将Uint16的高８位和低８位交换
        private ushort U32HighWord(uint x)
        {
            ushort highbit;

            highbit = (ushort)(x >> 16);
            highbit = Swap(highbit);

            return (highbit);
        }

        ///取Uint32的低１６位，并将Uint16的高８位和低８位交换
        private ushort U32LowWord(uint x)
        {
            ushort lowbit;
            lowbit = (ushort)(x & 0x0000ffff);
            lowbit = Swap(lowbit);
            return (lowbit);
        }
        //电动缸相关参数
        class ELECTRIC_CYCLINDER
        {
            public float F32AccessDistanceUnitMm = 70f;//电动缸行程
            public float F32LeadDistanceUnitMm = 5f;//电动缸导程
            public uint U32OneTurnPulseNum = 10000;//一圈脉冲数
            public float F32Gear = 1f;//减速比
        }
        ELECTRIC_CYCLINDER ElectricCylinder = new ELECTRIC_CYCLINDER();
        private void button1_Click(object sender, EventArgs e)
        {
            UdpAppData.I16HostTxPort = (short)8401;//主机 发送UDP数据端口号
            UdpAppData.I16MboxRxPort = (short)7408;//MBOX 接收UDP数据端口号
            UdpAppData.I16WhoAcceptCode = (ushort)ByteToWord((int)255, (int)255); //IP Group和IP Node合并为谁接收
            UdpAppData.I16WhoReplyCode = (ushort)ByteToWord((int)255, (int)255); //IP Group和IP Node合并为谁接收
            ElectricCylinder.F32AccessDistanceUnitMm = (float)70f; //电动缸行程
            ElectricCylinder.F32LeadDistanceUnitMm = (float)5f; //电动缸导程
            ElectricCylinder.U32OneTurnPulseNum = (uint)10000; //一圈脉冲数
            ElectricCylinder.F32Gear = (float)1f;//减速比
            HostUDPPort.Close();
            HostUDPPort = new UdpClient(UdpAppData.I16HostTxPort);
            IPEndPoint ipendpoint = new IPEndPoint(IPAddress.Any, UdpAppData.I16MboxRxPort);
            HostUDPPort.Connect("255.255.255.255", UdpAppData.I16MboxRxPort);
            
        }
        class UDPDataFunctionCode
        {
            //Read Reg Object Channel Code
            public static int FunctionCode_Report_DnReg = 0x1001;//MBOX上报功能码     声明实例变量   实例变量需创建
            public static int FunctionCode_Read_DnFnReg = 0x1101;//读寄存器功能码
            public static int FunctionCode_Write_DnFnReg = 0x1201;//写寄存器功能码
            public static int FunctionCode_AbsTimePlay = 0x1301;//绝对时间播放
            public static int FunctionCode_DeltaTimePlay = 0x1401;//相对时间播放
        }
        //UDP指令的通道号
        class UDPDataObiectChannel
        {
            //Read Reg Object Channel Code
            public static int ChannelCode_Read_DnReg = 0; //读Dn寄存器通道
            public static int ChannelCode_Read_FnReg = 1;//读Fn寄存器通道

            //Write Reg Object Channel Code
            public static int ChannelCode_Write_FnReg_NOSave = 0;//写Fn寄存器但掉电不保存通道
            public static int ChannelCode_Write_FnReg_Save = 1;//写Fn寄存器掉电保存通道
            public static int ChannelCode_Write_CXReg = 2;//写CX寄存器通道

            //Play Action Data（六轴数据支持1-6轴平台，建议在编程时直接使用6轴数据）
            public static int ChannelCode_Play_ActionData_Three = 0;//播放3轴数据通道
            public static int ChannelCode_Play_ActionData_Six = 1;//播放6轴数据通道
        }
        private void PlayActionCmd()
        {
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset000_UdpApp_ConfirmCode] = (short)Swap(UdpAppData.I16ConfirmCode); //确认码
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset001_UdpApp_PassWord] = (short)Swap(UdpAppData.I16PassCode); //通过码 
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset002_UdpApp_FunctionCode] = (short)Swap(UdpAppData.I16FunctionCode); //功能码 
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset003_UdpApp_ObjectChannel] = (short)Swap(UdpAppData.I16ChannelCode); //通道号
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset004_UdpApp_WhoAcceptCode] = (short)Swap(UdpAppData.I16WhoAcceptCode); //谁接收
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset005_UdpApp_WhoReplyCode] = (short)Swap(UdpAppData.I16WhoReplyCode);  //谁回复
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset006_UdpApp_PlayLineHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayLine);// 指令编号
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset007_UdpApp_PlayLineLow] = (short)U32LowWord((uint)UdpAppData.I32PlayLine);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset008_UdpApp_PlayTimeHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayTime); //时间
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset009_UdpApp_PlayTimeLow] = (short)U32LowWord((uint)UdpAppData.I32PlayTime);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset010_UdpApp_PlayXPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayXpos); //X位置
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset011_UdpApp_PlayXPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayXpos);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset012_UdpApp_PlayYPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayYpos); //Y位置
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset013_UdpApp_PlayYPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayYpos);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset014_UdpApp_PlayZPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayZpos); //Z位置
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset015_UdpApp_PlayZPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayZpos);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset016_UdpApp_PlayUPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayUpos);//U位置 
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset017_UdpApp_PlayUPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayUpos);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset018_UdpApp_PlayVPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayVpos); //V位置
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset019_UdpApp_PlayVPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayVpos);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset020_UdpApp_PlayWPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayWpos); //W位置
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset021_UdpApp_PlayWPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayWpos);
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset022_UdpApp_PlayBaseDo] = (short)Swap(UdpAppData.I16BaseDoutCode); //基本开关量
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset023_UdpApp_PlayDac1] = (short)Swap(UdpAppData.I16DacOneCode); // 模拟量1输出
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset024_UdpApp_PlayDac2] = (short)Swap(UdpAppData.I16DacTwoCode); // 模拟量2输出
            Buffer.BlockCopy(I16UdpTxBuffer, 0, U8UdpDataSend, 0, U8UdpDataSend.Length); // 将short数组转化为byte数组
            HostUDPPort.Send(U8UdpDataSend, (UDPDataWordOffset.WordOffset024_UdpApp_PlayDac2 + 1) * 2);  //发送UDP数据	
        }
        class UDPDataWordOffset
        {
            //  UDP Tx Buffer WordOffset 
            public static int WordOffset000_UdpApp_ConfirmCode = 0;
            public static int WordOffset001_UdpApp_PassWord = 1;
            public static int WordOffset002_UdpApp_FunctionCode = 2;
            public static int WordOffset003_UdpApp_ObjectChannel = 3;
            public static int WordOffset004_UdpApp_WhoAcceptCode = 4;
            public static int WordOffset005_UdpApp_WhoReplyCode = 5;

            public static int WordOffset006_UdpApp_RegStartAddress = 6;
            public static int WordOffset007_UdpApp_RegVisitNum = 7;
            public static int WordOffset008_UdpApp_RegVisitDataBase = 8;
            public static int WordOffset009_UdpApp_RegVisitErrorCode = 9;

            public static int WordOffset006_UdpApp_PlayLineHigh = 6;
            public static int WordOffset007_UdpApp_PlayLineLow = 7;
            public static int WordOffset008_UdpApp_PlayTimeHigh = 8;
            public static int WordOffset009_UdpApp_PlayTimeLow = 9;

            public static int WordOffset010_UdpApp_PlayXPosHigh = 10;
            public static int WordOffset011_UdpApp_PlayXPosLow = 11;

            public static int WordOffset012_UdpApp_PlayYPosHigh = 12;
            public static int WordOffset013_UdpApp_PlayYPosLow = 13;

            public static int WordOffset014_UdpApp_PlayZPosHigh = 14;
            public static int WordOffset015_UdpApp_PlayZPosLow = 15;

            public static int WordOffset016_UdpApp_PlayUPosHigh = 16;
            public static int WordOffset017_UdpApp_PlayUPosLow = 17;

            public static int WordOffset018_UdpApp_PlayVPosHigh = 18;
            public static int WordOffset019_UdpApp_PlayVPosLow = 19;

            public static int WordOffset020_UdpApp_PlayWPosHigh = 20;
            public static int WordOffset021_UdpApp_PlayWPosLow = 21;

            public static int WordOffset022_UdpApp_PlayBaseDo = 22;
            public static int WordOffset023_UdpApp_PlayDac1 = 23;
            public static int WordOffset024_UdpApp_PlayDac2 = 24;
            public static int WordOffset025_UdpApp_PlayExtDo = 25;
        }
        //任意位置
        System.Timers.Timer AnysportTimer = new System.Timers.Timer();//定时器设置（2）
       //定时器采用系统定时器
        private void button3_Click(object sender, EventArgs e)//定时器设置（3）
        {
            AnysportTimer.Interval = 600; //0.01*1000+0.5  定时器发生时间的间隔越小采样频率越高                                                      //定时器使能
            AnysportTimer.Enabled = true;
            textBox2.Clear();
           

        }
        //定义电动缸的初始长度
        float z1 = 0;
        float z2 = 0;
        float z3 = 0;
        float z4= 0;
        float z5 = 0;
        float z6 = 0;
        //
        private void AnysportTimeevent  (object source, ElapsedEventArgs e)
        {
         
            UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
            UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
            UdpAppData.I32PlayLine += 1;
            UdpAppData.I32PlayTime = 3000;//时间单位为毫秒  是指令的执行时间执行时间越长动作越慢
          
            float q = Convert.ToSingle(str);//将传感器传来的数据转换为浮点型。
            float force2 = (float)(250 * (q - 5.4));//电压信号转力
            float force = Math.Abs(force2);//显示力
            force1 = Convert.ToString(force);

            if (q > 5.44)
               {
                //阻抗控制 kd=100、300、30
               // double ZP1 = (500 * (q - 4.08)/*力*//10);
               double YP1 = (250 * (q - 5.4)/*力*// 5);

                float XP = Convert.ToSingle(x.Text);  //  l[0]                //相对初始位置运动的坐标
                float YP = Convert.ToSingle(y.Text)+(float)YP1;
                float ZP = Convert.ToSingle(z.Text);
                double YAWP = Convert.ToSingle(yaw.Text) * Math.PI / 180;
                double ROLLP = Convert.ToSingle(roll.Text) * Math.PI / 180;            //相对静平台的恣态绕Y轴
                double PITCHP = Convert.ToSingle(pitch.Text) * Math.PI / 180;
                float[] m = { XP, YP, ZP, (float)PITCHP, (float)ROLLP, (float)YAWP };
                nijie = FANJIE(m);
                // float  z=UdpAppData.I32PlayWpos;
                //float p1 = z - 50000;//第一版(第一组)
                //float  p1= (float)(z -(100000*(q-4.13)));//第二版（第二组）这是


                UdpAppData.I32PlayXpos = (int)(2000 * nijie[0]);//绝对位置
                UdpAppData.I32PlayYpos = (int)(2000 * nijie[1]);
                UdpAppData.I32PlayZpos = (int)(2000 * nijie[2]);
                UdpAppData.I32PlayUpos = (int)(2000 * nijie[3]);
                UdpAppData.I32PlayVpos = (int)(2000 * nijie[4]);
                UdpAppData.I32PlayWpos = (int)(2000 * nijie[5]);


            }
                else
                {
                float XP = Convert.ToSingle(x.Text);  //  l[0]                //相对初始位置运动的坐标
                float YP = Convert.ToSingle(y.Text);
                float ZP = Convert.ToSingle(z.Text);
                double YAWP = Convert.ToSingle(yaw.Text) * Math.PI / 180;
                double ROLLP = Convert.ToSingle(roll.Text) * Math.PI / 180;            //相对静平台的恣态绕Y轴
                double PITCHP = Convert.ToSingle(pitch.Text) * Math.PI / 180;
                float[] m = { XP, YP, ZP, (float)PITCHP, (float)ROLLP, (float)YAWP };
                nijie = FANJIE(m);
                 UdpAppData.I32PlayXpos = (int)(2000 *nijie[0]);//绝对位置
                    UdpAppData.I32PlayYpos = (int)(2000 *nijie[1]);
                    UdpAppData.I32PlayZpos = (int)(2000 *nijie[2]);
                    UdpAppData.I32PlayUpos = (int)(2000 *nijie[3]);
                    UdpAppData.I32PlayVpos = (int)(2000*nijie[4]);
                    UdpAppData.I32PlayWpos = (int)(2000 *nijie[5]);
             
            }
                PlayActionCmd();
            z1 = UdpAppData.I32PlayXpos / 2000;//一号缸的长度摄像头左边
            z2 = UdpAppData.I32PlayYpos / 2000;
            z3 = UdpAppData.I32PlayZpos / 2000;
            z4 = UdpAppData.I32PlayUpos / 2000;
            z5 = UdpAppData.I32PlayVpos / 2000;
            z6 = UdpAppData.I32PlayWpos / 2000;
            //这是沿z走90mm显示的缸长（这采出的数据和串口一样）
            if (gangchangshujujieshou)
            {
                string L1 = string.Format("{0:F}", z1);//保留两位小数
                textBox1.AppendText(L1);
                textBox1.AppendText(System.Environment.NewLine);//换行
            }
            if (gangchangshujujieshou)
            {
                string L2 = string.Format("{0:F}", z2);//保留两位小数
                textBox4.AppendText(L2);
                textBox4.AppendText(System.Environment.NewLine);//换行
            }
            if (gangchangshujujieshou)
            {
                string L3 = string.Format("{0:F}", z3);//保留两位小数
                textBox5.AppendText(L3);
                textBox5.AppendText(System.Environment.NewLine);//换行
            }
            if (gangchangshujujieshou)
            {
                string L4 = string.Format("{0:F}", z4);//保留两位小数
                textBox6.AppendText(L4);
                textBox6.AppendText(System.Environment.NewLine);//换行
            }
            if (gangchangshujujieshou)
            {
                string L5 = string.Format("{0:F}", z5);//保留两位小数
                textBox7.AppendText(L5);
                textBox7.AppendText(System.Environment.NewLine);//换行
            }
            if (gangchangshujujieshou)
            {
                string L6 = string.Format("{0:F}", z6);//保留两位小数
                textBox8.AppendText(L6);
                textBox8.AppendText(System.Environment.NewLine);//换行
            }

        }
        
       
        //写寄存器指令,平台复位指令
        private void WriteRegCmd(ushort ChannelCode, ushort RegAddress, short RegValue)
        {
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset000_UdpApp_ConfirmCode] = (short)Swap(UdpAppData.I16ConfirmCode); //确认码
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset001_UdpApp_PassWord] = (short)Swap(UdpAppData.I16PassCode); //通过码 
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset002_UdpApp_FunctionCode] = (short)Swap(UdpAppData.I16FunctionCode); //功能码 
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset003_UdpApp_ObjectChannel] = (short)Swap(ChannelCode); //通道号
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset004_UdpApp_WhoAcceptCode] = (short)Swap(UdpAppData.I16WhoAcceptCode); //谁接收
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset005_UdpApp_WhoReplyCode] = (short)Swap(UdpAppData.I16WhoReplyCode);  //谁回复

            I16UdpTxBuffer[UDPDataWordOffset.WordOffset006_UdpApp_RegStartAddress] = (short)Swap(RegAddress);//寄存器地址
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset007_UdpApp_RegVisitNum] = (short)Swap(0x0001);//寄存器个数
            I16UdpTxBuffer[UDPDataWordOffset.WordOffset008_UdpApp_RegVisitDataBase] = (short)Swap((ushort)RegValue); //寄存器的值 
            Buffer.BlockCopy(I16UdpTxBuffer, 0, U8UdpDataSend, 0, U8UdpDataSend.Length); // 将short数组转化为byte数组
            HostUDPPort.Send(U8UdpDataSend, (UDPDataWordOffset.WordOffset008_UdpApp_RegVisitDataBase + 1) * 2);  //发送UDP数据	
        }
        class MBOXCXRegAddress
        {
            //MBOX CX Reg Address
            public static int CX_ID_MboxPlayControlWord = 0x0;//平台复位寄存器，声明实例变量   寄存器地址复位
            public static int CX_ID_MboxPlayControlWord1 = 0x0090;//平台复位寄存器，声明实例变量   寄存器地址平台急停地址

        }
        private void button4_Click(object sender, EventArgs e)
        {
            UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_Write_DnFnReg;
            UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Write_CXReg;
            WriteRegCmd(UdpAppData.I16ChannelCode, (ushort)MBOXCXRegAddress.CX_ID_MboxPlayControlWord, 0);
        }
        private void 平台急停_Click(object sender, EventArgs e)
        {
            UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_Write_DnFnReg;
            UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Write_FnReg_NOSave;
            WriteRegCmd(UdpAppData.I16ChannelCode, (ushort)MBOXCXRegAddress.CX_ID_MboxPlayControlWord1, 0001);
        }
        private void 急停取消_Click(object sender, EventArgs e)
        {
            UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_Write_DnFnReg;
            UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Write_FnReg_NOSave;
            WriteRegCmd(UdpAppData.I16ChannelCode, (ushort)MBOXCXRegAddress.CX_ID_MboxPlayControlWord1, 0000);
        }

        //正弦运动
        System.Timers.Timer SineTimer = new System.Timers.Timer();//定时器采用系统定时器
      // System.Timers.Timer SineTimer = new System.Timers.Timer();
        class SINE_ACT_DATA
        {
            public float F32SamplingPeriods = 0.01f;

            public float F32PeakMm = 80.0f;
            public float F32FrequencyHz = 0.1f;

            public float F32SineDataMm = 0.0f;

            public float F32XMm = 0.0f;
            public float F32YMm = 0.0f;
            public float F32ZMm = 0.0f;
            public float F32UMm = 0.0f;
            public float F32VMm = 0.0f;
            public float F32WMm = 0.0f;

            public short I16Delta = 0;
            public int I32Iine = 0;
            public int I32CyclePoint = 0;
        }

        SINE_ACT_DATA SineActData = new SINE_ACT_DATA();
        private void button2_Click(object sender, EventArgs e)
        {
            SineActData.F32PeakMm = (float)50; //正弦峰值 sineActdata是一个实体变量 SINE_ACT_DATA是类名class
            SineActData.F32FrequencyHz = (float)0.5f;//正弦波频率
            SineActData.F32SamplingPeriods = (float)0.01f;//正弦波采样周期

            if (SineActData.F32PeakMm > ElectricCylinder.F32AccessDistanceUnitMm)
            {
                MessageBox.Show("正弦峰值大于电动缸行程，请重新设置正弦峰值或电动缸行程!", "警告");
            }
            else
            {
                //设置引发时间的时间间隔为正弦波采样周期
                SineTimer.Interval = SineActData.F32SamplingPeriods * 1000 + 0.5;//0.01*1000+0.5
                //定时器使能
                SineTimer.Enabled = true;
            }
        }
        //定时器定时发送正弦数据
        private void SineTimeEvent(object source, ElapsedEventArgs e)
        {
            short m = 0;

            if (SineActData.I16Delta >= SineActData.I32CyclePoint)
            {
                SineActData.I16Delta = (short)(SineActData.I16Delta - SineActData.I32CyclePoint);
            }

            m = (SineActData.I16Delta)++;
            //SineActData.F32XMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            //SineActData.F32YMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            //SineActData.F32ZMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            //SineActData.F32UMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            //SineActData.F32VMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            //SineActData.F32WMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            SineActData.F32XMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            SineActData.F32YMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            SineActData.F32ZMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            SineActData.F32UMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            SineActData.F32VMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));
            SineActData.F32WMm = SineDataMm(SineActData.F32PeakMm, SineActData.F32FrequencyHz, (m * (SineActData.F32SamplingPeriods)));

            //相对时间播放
            UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
            UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
            UdpAppData.I32PlayLine += 1;
            UdpAppData.I32PlayTime = (int)(SineActData.F32SamplingPeriods * 1000 + 0.5);//时间单位为毫秒,I32PlayTime为指令的执行时间
            UdpAppData.I32PlayXpos = (int)(SineActData.F32XMm * ElectricCylinder.U32OneTurnPulseNum * ElectricCylinder.F32Gear / ElectricCylinder.F32LeadDistanceUnitMm);
            UdpAppData.I32PlayYpos = (int)(SineActData.F32YMm * ElectricCylinder.U32OneTurnPulseNum * ElectricCylinder.F32Gear / ElectricCylinder.F32LeadDistanceUnitMm);
            UdpAppData.I32PlayZpos = (int)(SineActData.F32ZMm * ElectricCylinder.U32OneTurnPulseNum * ElectricCylinder.F32Gear / ElectricCylinder.F32LeadDistanceUnitMm);
            UdpAppData.I32PlayUpos = (int)(SineActData.F32UMm * ElectricCylinder.U32OneTurnPulseNum * ElectricCylinder.F32Gear / ElectricCylinder.F32LeadDistanceUnitMm);
            UdpAppData.I32PlayVpos = (int)(SineActData.F32VMm * ElectricCylinder.U32OneTurnPulseNum * ElectricCylinder.F32Gear / ElectricCylinder.F32LeadDistanceUnitMm);
            UdpAppData.I32PlayWpos = (int)(SineActData.F32WMm * ElectricCylinder.U32OneTurnPulseNum * ElectricCylinder.F32Gear / ElectricCylinder.F32LeadDistanceUnitMm);
            PlayActionCmd();
        }

        //产生正弦信号
        
        string force1 = "0";//将force1作为字符串显示到textbox;

        float sinedata = 0;//
        private float SineDataMm(float peak, float frequency, float t)//走多少毫米（sindata）
        {


        float q = Convert.ToSingle(str);//将传感器传来的数据转换为浮点型。
          

            float force2 = (float)(500 * (q - 4.14));//电压信号转力 
            float force = Math.Abs(force2);//显示力

          
            force1 = Convert.ToString(force);
           

           

            if (q>4.6)
            {
                float z = UdpAppData.I32PlayWpos;
                float p2 = z - 50000;
                UdpAppData.I32PlayXpos = (int)p2;
                UdpAppData.I32PlayYpos = (int)p2;
                UdpAppData.I32PlayZpos = (int)p2;
                UdpAppData.I32PlayVpos = (int)p2;
                UdpAppData.I32PlayUpos = (int)p2;
                UdpAppData.I32PlayWpos = (int)p2;



            }
            else
            {
               
                sinedata = (float)((float)((peak / 2) + 45 * Math.Sin(2 * Math.PI * frequency * (t-0.01) + 270 * Math.PI / 180)));
                

            }
            if (gangchangshujujieshou)
            {
                string gangchang = string.Format ("{0:F}",sinedata);//保留两位小数
                textBox1.AppendText(gangchang);
                textBox1.AppendText(System.Environment.NewLine);//换行
            }
            return sinedata;

        }

        private void 取消正弦运动_Click(object sender, EventArgs e)
        {
            SineTimer.Enabled = false;
        }
        public byte[] commend = new byte[10];

        private void groupBox1_Enter(object sender, EventArgs e)
        {

        }

        private void panel2_Paint(object sender, PaintEventArgs e)
        {

        }

        private void cmb_databits_SelectedIndexChanged(object sender, EventArgs e)
        {

        }

        private void textBox3_TextChanged(object sender, EventArgs e)
        {

        }

        private void label12_Click(object sender, EventArgs e)
        {

        }

        private void checkBox1_CheckedChanged(object sender, EventArgs e)
        {

        }

        private void label18_Click(object sender, EventArgs e)
        {

        }













        /* #region NPOI导出到Excel 
          //NPOI导出到Excel(故障码)
          public ActionResult ExportToExcel(string DTC)
          {
              int FaultCodeID = 0;//故障码ID

              //查询数据
              var listFaultInfo = listFaultCodePush(FaultCodeID, DTC);
              List<PlatformClass> listExaminee = listFaultInfo;
              //二：代码创建一个Excel表格（这里称为工作簿）
              //创建Excel文件的对象 工作簿(调用NPOI文件)
              HSSFWorkbook excelBook = new HSSFWorkbook();
              ICellStyle style1 = excelBook.CreateCellStyle();//声明style1对象，设置Excel表格的样式
              ICellStyle style2 = excelBook.CreateCellStyle();
              ICellStyle style3 = excelBook.CreateCellStyle();
              IFont font = excelBook.CreateFont();
              font.Color = IndexedColors.RED.Index;
              style3.SetFont(font);
              style1.Alignment = NPOI.SS.UserModel.HorizontalAlignment.Justify;//两端自动对齐（自动换行）
              style1.VerticalAlignment = VerticalAlignment.Center;
              style2.Alignment = NPOI.SS.UserModel.HorizontalAlignment.Center;
              style2.VerticalAlignment = VerticalAlignment.Center;
              style3.Alignment = NPOI.SS.UserModel.HorizontalAlignment.Center;
              style3.VerticalAlignment = VerticalAlignment.Center;
              //创建Excel工作表 Sheet=故障码信息
              ISheet sheet1 = excelBook.CreateSheet("故障码信息");
              //给Sheet(故障码信息）添加第一行的头部标题
              IRow row1 = sheet1.CreateRow(0);
              //给标题的每一个单元格赋值
              row1.CreateCell(0).SetCellValue("故障码DTC");//0
              row1.CreateCell(1).SetCellValue("故障码(hex)");//1
              row1.CreateCell(2).SetCellValue("故障码英文描述");//2
              row1.CreateCell(3).SetCellValue("故障码中文描述");//3
              row1.CreateCell(4).SetCellValue("故障码运行条件");//4
              row1.CreateCell(5).SetCellValue("故障码设置条件");//5
              row1.CreateCell(6).SetCellValue("故障码设置时发生的操作");//6
              row1.CreateCell(7).SetCellValue("故障恢复条件");//7
              row1.CreateCell(8).SetCellValue("激近故障灯原则");//8
              row1.CreateCell(9).SetCellValue("熄灭故障灯原则");//9
              row1.CreateCell(10).SetCellValue("清除故障码条件");//10
              row1.GetCell(0).CellStyle = style2;//初始化设置样式
              row1.GetCell(1).CellStyle = style2;//初始化设置样式
              row1.GetCell(2).CellStyle = style2;//初始化设置样式
              row1.GetCell(3).CellStyle = style2;//初始化设置样式
              row1.GetCell(4).CellStyle = style2;//初始化设置样式
              row1.GetCell(5).CellStyle = style2;//初始化设置样式
              row1.GetCell(6).CellStyle = style2;//初始化设置样式
              row1.GetCell(7).CellStyle = style2;//初始化设置样式
              row1.GetCell(8).CellStyle = style2;//初始化设置样式
              row1.GetCell(9).CellStyle = style2;//初始化设置样式
              row1.GetCell(10).CellStyle = style2;//初始化设置样式
              sheet1.SetColumnWidth(0, 10 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(0, 10 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(0, 10 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(0, 10 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(1, 10 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(0, 10 * 256);//初始化设置样式
              sheet1.SetColumnWidth(2, 17 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(3, 30 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(4, 34 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(5, 20 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(6, 26 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(7, 29 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(8, 14 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(9, 14 * 256);//初始化设置宽度
              sheet1.SetColumnWidth(10, 99 * 256);//初始化设置宽度
              //添加数据行：将表格数据逐步写入sheet1各个行中（也就是给每一个单元格赋值）
              for (int i = 0; i < listExaminee.Count; i++)
              {
                  //sheet1.CreateRow(i).
                  //创建行
                  IRow rowTemp = sheet1.CreateRow(i + 1);
                  rowTemp.Height = 62 * 20;
                  //故障码DTC
                  rowTemp.CreateCell(0).SetCellValue(listFaultInfo[i].DTC);
                  //故障码(hex)
                  rowTemp.CreateCell(1).SetCellValue(listFaultInfo[i].HEX);
                  //故障码英文描述
                  rowTemp.CreateCell(2).SetCellValue(listFaultInfo[i].EfaultCode);
                  //故障码中文描述
                  rowTemp.CreateCell(3).SetCellValue(listFaultInfo[i].CfaultCode);
                  ///故障码运行条件
                  rowTemp.CreateCell(4).SetCellValue(listFaultInfo[i].OperatingCondition.Replace("<br>", " "));
                  //故障码设置条件
                  rowTemp.CreateCell(5).SetCellValue(listFaultInfo[i].SetCriteria.Replace("<br>", " "));
                  //故障码设置时发生的操作
                  rowTemp.CreateCell(6).SetCellValue(listFaultInfo[i].SetingOperation);
                  //故障恢复条件
                  rowTemp.CreateCell(7).SetCellValue(listFaultInfo[i].RestoreConditions);
                  //激近故障灯原则
                  rowTemp.CreateCell(8).SetCellValue(listFaultInfo[i].ActivateRule);
                  //熄灭故障灯原则
                  rowTemp.CreateCell(9).SetCellValue(listFaultInfo[i].OutRule);
                  //清除故障码条件
                  rowTemp.CreateCell(10).SetCellValue(listFaultInfo[i].CleanCondition.Replace("<br>", " "));
                  rowTemp.GetCell(0).CellStyle = style3;
                  rowTemp.GetCell(1).CellStyle = style3;
                  rowTemp.GetCell(2).CellStyle = style2;
                  rowTemp.GetCell(3).CellStyle = style2;
                  rowTemp.GetCell(4).CellStyle = style1;
                  rowTemp.GetCell(5).CellStyle = style1;
                  rowTemp.GetCell(6).CellStyle = style1;
                  rowTemp.GetCell(7).CellStyle = style1;
                  rowTemp.GetCell(8).CellStyle = style2;
                  rowTemp.GetCell(9).CellStyle = style2;
                  rowTemp.GetCell(10).CellStyle = style1;
              }
              //输出的文件名称
              string fileName = "故障码信息" + DateTime.Now.ToString("yyyy-MM-dd-HH-mm-ss-ffff") + ".xls";
              //把Excel转为流，输出
              //创建文件流
              System.IO.MemoryStream bookStream = new System.IO.MemoryStream();
              //将工作薄写入文件流
              excelBook.Write(bookStream);

              //输出之前调用Seek（偏移量，游标位置) 把0位置指定为开始位置
              bookStream.Seek(0, System.IO.SeekOrigin.Begin);
              //Stream对象,文件类型,文件名称
              return File(bookStream, "application/vnd.ms-excel", fileName);
          }
          #endregion
        */
        //反解算法
        private static float[] FANJIE(float[] l)
        {
            float XP = 0;                    // 动平台  相对 静平台  的初始位置坐标
            float YP = 0;
            float ZP = 600;

            float[,] P = new float[3, 1] { { l[0] + XP }, { l[1] + YP }, { l[2] + ZP } };
            int R = 182;                  // 动平台铰点的外接圆半径
            int r = 221;                  // 静平台铰点的外接圆半径
            double up_angle0 = 45 * Math.PI / 180;     //动平台铰点的安装角度
            double up_angle1 = 75 * Math.PI / 180;
            double up_angle2 = 165 * Math.PI / 180;
            double up_angle3 = -165 * Math.PI / 180;
            double up_angle4 = -75 * Math.PI / 180;
            double up_angle5 = -45 * Math.PI / 180;

            double down_angle0 = 21 * Math.PI / 180;     //静平台铰点的安装角度
            double down_angle1 = 99 * Math.PI / 180;
            double down_angle2 = 141 * Math.PI / 180;
            double down_angle3 = -141 * Math.PI / 180;
            double down_angle4 = -99 * Math.PI / 180;
            double down_angle5 = -21 * Math.PI / 180;
            // ----------动平台的6个铰点，在动平台坐标系中的位置矢量-------------------- -
            float[,] bR1 = new float[3, 1] { { (float)(R * Math.Cos(up_angle0)) }, { (float)(R * Math.Sin(up_angle0)) }, { 0 } };
            float[,] bR2 = new float[3, 1] { { (float)(R * Math.Cos(up_angle1)) }, { (float)(R * Math.Sin(up_angle1)) }, { 0 } };
            float[,] bR3 = new float[3, 1] { { (float)(R * Math.Cos(up_angle2)) }, { (float)(R * Math.Sin(up_angle2)) }, { 0 } };
            float[,] bR4 = new float[3, 1] { { (float)(R * Math.Cos(up_angle3)) }, { (float)(R * Math.Sin(up_angle3)) }, { 0 } };
            float[,] bR5 = new float[3, 1] { { (float)(R * Math.Cos(up_angle4)) }, { (float)(R * Math.Sin(up_angle4)) }, { 0 } };
            float[,] bR6 = new float[3, 1] { { (float)(R * Math.Cos(up_angle5)) }, { (float)(R * Math.Sin(up_angle5)) }, { 0 } };
            // ----------静平台的6个铰点，在静平台坐标系中的位置矢量-------------------- -
            float[,] Br1 = new float[3, 1] { { (float)(r * Math.Cos(down_angle0)) }, { (float)(r * Math.Sin(down_angle0)) }, { 0 } };
            float[,] Br2 = new float[3, 1] { { (float)(r * Math.Cos(down_angle1)) }, { (float)(r * Math.Sin(down_angle1)) }, { 0 } };
            float[,] Br3 = new float[3, 1] { { (float)(r * Math.Cos(down_angle2)) }, { (float)(r * Math.Sin(down_angle2)) }, { 0 } };
            float[,] Br4 = new float[3, 1] { { (float)(r * Math.Cos(down_angle3)) }, { (float)(r * Math.Sin(down_angle3)) }, { 0 } };
            float[,] Br5 = new float[3, 1] { { (float)(r * Math.Cos(down_angle4)) }, { (float)(r * Math.Sin(down_angle4)) }, { 0 } };
            float[,] Br6 = new float[3, 1] { { (float)(r * Math.Cos(down_angle5)) }, { (float)(r * Math.Sin(down_angle5)) }, { 0 } };


            float[,] rotz = new float[3, 3] { { (float)Math.Cos(l[5]), (float)(-1 * Math.Sin(l[5])), 0 }, { (float)Math.Sin(l[5]), (float)Math.Cos(l[5]), 0 }, { 0, 0, 1 } };//YAW
            float[,] roty = new float[3, 3] { { (float)Math.Cos(l[4]), 0, (float)Math.Sin(l[4]) }, { 0, 1, 0 }, { (float)-Math.Sin(l[4]), 0, (float)Math.Cos(l[4]) } };//pitch
            float[,] rotx = new float[3, 3] { { 1, 0, 0 }, { 0, (float)Math.Cos(l[3]), (float)-Math.Sin(l[3]) }, { 0, (float)Math.Sin(l[3]), (float)Math.Cos(l[3]) } };//roll





            //TransM[3, 3] = rotz[3, 3] * roty[3, 3] * rotx[3, 3]; //XYZ旋转矩阵
            float[,] TransM1 = new float[3, 3];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        TransM1[i, j] += rotz[i, k] * roty[k, j];
                    }
                }
            }
            float[,] TransM = new float[3, 3];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        TransM[i, j] += TransM1[i, k] * rotx[k, j];
                    }
                }
            }

            //求杆
            //****************************************************/
            float[,] br1 = new float[3, 1];
            // br1[3, 1] = TransM[3, 3] * bR1[3, 1] + P[3, 1];
            float[,] zhiongjian1 = new float[3, 1];//定义中间变量 TransM[3, 3] * bR1[3, 1]
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        zhiongjian1[i, j] += TransM[i, k] * bR1[k, j];
                    }
                }
            }
            //  br1[3, 1] = zhiongjian1 [3,1] + P[3, 1];

            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    br1[i, j] = zhiongjian1[i, j] + P[i, j];

                }
            }
            /***********************************************************2**/

            float[,] br2 = new float[3, 1];
            // br2[3, 1] = TransM[3, 3] * bR2[3, 1] + P[3, 1];
            float[,] zhiongjian2 = new float[3, 1];//定义中间变量 TransM[3, 3] * bR1[3, 1]
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        zhiongjian2[i, j] += TransM[i, k] * bR2[k, j];
                    }
                }
            }


            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    br2[i, j] = zhiongjian2[i, j] + P[i, j];

                }
            }
            /***********************************************************3**********/

            float[,] br3 = new float[3, 1];
            // br3[3, 1] = TransM[3, 3] * bR3[3, 1] + P[3, 1];
            float[,] zhiongjian3 = new float[3, 1];//定义中间变量 TransM[3, 3] * bR1[3, 1]
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        zhiongjian3[i, j] += TransM[i, k] * bR3[k, j];
                    }
                }
            }
            //  br1[3, 1] = zhiongjian1 [3,1] + P[3, 1];

            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    br3[i, j] = zhiongjian3[i, j] + P[i, j];

                }
            }
            /**********************************4*********************/

            float[,] br4 = new float[3, 1];
            // br4[3, 1] = TransM[3, 3] * bR4[3, 1] + P[3, 1];
            float[,] zhiongjian4 = new float[3, 1];//定义中间变量 TransM[3, 3] * bR1[3, 1]
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        zhiongjian4[i, j] += TransM[i, k] * bR4[k, j];
                    }
                }
            }


            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    br4[i, j] = zhiongjian4[i, j] + P[i, j];

                }
            }
            /******************************************************************5**************/
            float[,] br5 = new float[3, 1];
            //     br5[3, 1] = TransM[3, 3] * bR5[3, 1] + P[3, 1];
            float[,] zhiongjian5 = new float[3, 1];//定义中间变量 TransM[3, 3] * bR1[3, 1]
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        zhiongjian5[i, j] += TransM[i, k] * bR5[k, j];
                    }
                }
            }
            //  br1[3, 1] = zhiongjian1 [3,1] + P[3, 1];

            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    br5[i, j] = zhiongjian5[i, j] + P[i, j];

                }
            }
            /*************************************************************6*********************/
            float[,] br6 = new float[3, 1];
            //    br6[3, 1] = TransM[3, 3] * bR6[3, 1] + P[3, 1];
            float[,] zhiongjian6 = new float[3, 1];//定义中间变量 TransM[3, 3] * bR1[3, 1]
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        zhiongjian6[i, j] += TransM[i, k] * bR6[k, j];
                    }
                }
            }


            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    br6[i, j] = zhiongjian6[i, j] + P[i, j];

                }
            }


            //--动平台的6个铰点位置矢量，减去，静平台的6个铰点位置矢量，得到每个杆长矢量
            /* L1 = br1 - Br1;
             L2 = br2 - Br2;
             L3 = br3 - Br3;
             L4 = br4 - Br4;
             L5 = br5 - Br5;
             L6 = br6 - Br6;*/

            float[,] L1 = new float[3, 1];
            //   L1[3, 1] = br1[3, 1] - Br1[3, 1];
            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    L1[i, j] = br1[i, j] - Br1[i, j];

                }
            }
            /***************************************/
            float[,] L2 = new float[3, 1];
            // L2[3, 1] = br2[3, 1] - Br2[3, 1];
            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    L2[i, j] = br2[i, j] - Br2[i, j];

                }
            }

            /**********************************************************/

            float[,] L3 = new float[3, 1];
            //L3[3, 1] = br3[3, 1] - Br3[3, 1];
            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    L3[i, j] = br3[i, j] - Br3[i, j];

                }
            }
            /**********************************************************/

            float[,] L4 = new float[3, 1];
            //  L4[3, 1] = br4[3, 1] - Br4[3, 1];
            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    L4[i, j] = br4[i, j] - Br4[i, j];

                }
            }
            /**********************************************/

            float[,] L5 = new float[3, 1];
            // L5[3, 1] = br5[3, 1] - Br5[3, 1];
            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    L5[i, j] = br5[i, j] - Br5[i, j];

                }
            }
            /*************************************************************/
            float[,] L6 = new float[3, 1];
            // L6[3, 1] = br6[3, 1] - Br6[3, 1];
            for (int j = 0; j < 1; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    L6[i, j] = br6[i, j] - Br6[i, j];

                }
            }


            // -----------求模，得到每个杆的杆长---------------------------------------- -
            float L;
            L = 607;
            float LenL1;
            LenL1 = (float)(Math.Sqrt(L1[0, 0] * L1[0, 0] + L1[1, 0] * L1[1, 0] + L1[2, 0] * L1[2, 0]) - L);
            float LenL2;
            LenL2 = (float)(Math.Sqrt(L2[0, 0] * L2[0, 0] + L2[1, 0] * L2[1, 0] + L2[2, 0] * L2[2, 0]) - L);
            float LenL3;
            LenL3 = (float)(Math.Sqrt(L3[0, 0] * L3[0, 0] + L3[1, 0] * L3[1, 0] + L3[2, 0] * L3[2, 0]) - L);
            float LenL4;
            LenL4 = (float)(Math.Sqrt(L4[0, 0] * L4[0, 0] + L4[1, 0] * L4[1, 0] + L4[2, 0] * L4[2, 0]) - L);
            float LenL5;
            LenL5 = (float)(Math.Sqrt(L5[0, 0] * L5[0, 0] + L5[1, 0] * L5[1, 0] + L5[2, 0] * L5[2, 0]) - L);
            float LenL6;
            LenL6 = (float)(Math.Sqrt(L6[0, 0] * L6[0, 0] + L6[1, 0] * L6[1, 0] + L6[2, 0] * L6[2, 0]) - L);
            var g = new[] { LenL1, LenL2, LenL3, LenL4, LenL5, LenL6 };
            var h = new[] { LenL1 - 88, LenL2 - 88, LenL3 - 88, LenL4 - 88, LenL5 - 88, LenL6 - 88 };
            return g;



        }
        public static float[] nijie; //将反解出来的缸长给anysportevent；
        public void button10_Click(object sender, EventArgs e)//发送按钮
        {
            gangchangshujujieshou = true;
            float XP = 30;// Convert .ToSingle (x.Text );  //  l[0]                //相对初始位置运动的坐标
            float YP = -100;// Convert.ToSingle(y.Text);
            float ZP = 50;// Convert.ToSingle(z.Text);
            double YAWP = 0;// Convert .ToSingle (yaw.Text ) * Math.PI / 180;
            double ROLLP = 0;// Convert.ToSingle(roll.Text) * Math.PI / 180;            //相对静平台的恣态绕Y轴
            double PITCHP = 10 * Math.PI / 180;//Convert.ToSingle(pitch .Text) * Math.PI / 180;
            float[] m = { XP, YP, ZP, (float)PITCHP, (float)ROLLP, (float)YAWP };
            nijie = FANJIE(m);
            AnysportTimer.Interval = 1000; //0.01*1000+0.5  定时器发生时间的间隔越小采样频率越高                                                      //定时器使能
           AnysportTimer.Enabled = true;
            

        }
        private void button14_Click(object sender, EventArgs e)
        {
            gangchangshujujieshou = true;
            float XP = 30;// Convert .ToSingle (x.Text );  //  l[0]                //相对初始位置运动的坐标
            float YP = 100;// Convert.ToSingle(y.Text);
            float ZP = 50;// Convert.ToSingle(z.Text);
            double YAWP = 0;// Convert .ToSingle (yaw.Text ) * Math.PI / 180;
            double ROLLP = 0;// Convert.ToSingle(roll.Text) * Math.PI / 180;            //相对静平台的恣态绕Y轴
            double PITCHP = -10 * Math.PI / 180;//Convert.ToSingle(pitch .Text) * Math.PI / 180;
            float[] m = { XP, YP, ZP, (float)PITCHP, (float)ROLLP, (float)YAWP };
            nijie = FANJIE(m);
            //AnysportTimer.Interval = 1000; //0.01*1000+0.5  定时器发生时间的间隔越小采样频率越高                                                      //定时器使能
            //AnysportTimer.Enabled = true;
            UdpAppData.I32PlayXpos = (int)(2000 * nijie[0]);//绝对位置
            UdpAppData.I32PlayYpos = (int)(2000 * nijie[1]);
            UdpAppData.I32PlayZpos = (int)(2000 * nijie[2]);
            UdpAppData.I32PlayUpos = (int)(2000 * nijie[3]);
            UdpAppData.I32PlayVpos = (int)(2000 * nijie[4]);
            UdpAppData.I32PlayWpos = (int)(2000 * nijie[5]);
            PlayActionCmd();


        }

        private void button11_Click(object sender, EventArgs e)//反解停止按钮
        {
            AnysportTimer.Enabled = false;
           

        }

        private void textBox4_TextChanged(object sender, EventArgs e)
        {

        }

        private void 六轴控制_Load(object sender, EventArgs e)
        {

        }

        private void groupBox9_Enter(object sender, EventArgs e)
        {

        }

        private void textBoxγ_TextChanged(object sender, EventArgs e)
        {

        }

        private void yaw_TextChanged(object sender, EventArgs e)
        {

        }

        
    }
}
